/**
 * Delta X Firmware
 * Copyright (c) 2020 DeltaXFirmware [https://github.com/deltaxrobot/Delta-X-Firmware]
 *
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
// DeltaKinematics.h

#ifndef _DELTAKINEMATICS_h
#define _DELTAKINEMATICS_h

#if defined(ARDUINO) && ARDUINO >= 100
#include "arduino.h"
#else

#include "WProgram.h"

#endif

#include "Constants.h"

using namespace std;

/**
 * 以下是三角函数的常量定义
 */
//trigonometric constants
const double tan30 = 1 / sqrt(3);
const double tan30x05 = 0.5 / sqrt(3);
const double tan60 = sqrt(3);
const double sin30 = 0.5;
const double cos30 = sqrt(3) / 2;
const double cos120 = -0.5;
const double sin120 = sqrt(3) / 2;

/**
 * Delta运动学处理类
 */
class DeltaKinematicsClass {
protected:


public:
    /**
     * 初始化
     */
    void init();

    /**
     * 运动学正解
     * @param angleposition 各个大臂的角度
     * @param point 当前末端的坐标
     * @return 是否执行成功
     */
    bool ForwardKinematicsCalculations(Angle angleposition, Point &point);

    /**
     * 运动学逆解
     * @param point 当前末端的坐标
     * @param angleposition 各个大臂的角度
     * @return 是否执行成功
     */
    bool InverseKinematicsCalculations(Point point, Angle &angleposition);

private:
    /**
     * 计算在选择的平面上（或者末端、大臂所在的平面）指定点到相应大臂的角度
     * @param x0 点的x坐标
     * @param y0 点的y坐标
     * @param z0 点的z坐标
     * @param theta 角度
     * @return 是否计算成功
     */
    bool AngleThetaCalculations(float x0, float y0, float z0, float &theta);

    float _y0_; // 顶座半径
    float _y1_; // 末端半径
    float RD_RF_Pow2; // L^2
    float RD_RE_Pow2; // l^2
};

/**
 * 外部声明
 */
extern DeltaKinematicsClass DeltaKinematics;

#endif

